import launch
import launch_ros
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

def generate_launch_description():

    # 声明偏移参数（添加这些配置）
    offset_x_arg = DeclareLaunchArgument(
        'offset_x',
        default_value='0.0',
        description='X-axis position offset'
    )
    offset_y_arg = DeclareLaunchArgument(
        'offset_y',
        default_value='0.0',
        description='Y-axis position offset'
    )
    offset_z_arg = DeclareLaunchArgument(
        'offset_z',
        default_value='0.0',
        description='Z-axis position offset'
    )

    new_cmd_vel = LaunchConfiguration('new_topic')

    cmd_vel_new = DeclareLaunchArgument(
        'new_topic',
        default_value='/mavros/setpoint_velocity/cmd_vel_unstamped',  # 默认值
        description='The name of the new topic'
    )
    
    odomzuzhuang_dir = get_package_share_directory(
        'odom_zuzhuang')
   
    urdf2tf = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [odomzuzhuang_dir,'/urdf2tf.launch.py']),
    )
    
    mavros2odom = launch_ros.actions.Node(
        package='odom_zuzhuang',
        executable='mavros2odom',
        output='screen',
        parameters=[{
            'frame_id': 'odom',
            'child_frame_id': 'base_footprint',
            # 添加偏移参数（通过LaunchConfiguration获取）
            'offset_x': LaunchConfiguration('offset_x'),
            'offset_y': LaunchConfiguration('offset_y'),
            'offset_z': LaunchConfiguration('offset_z')
        }]
    )

    odom2tf = launch_ros.actions.Node(
        package='odom_zuzhuang',
        executable='odom2tf',
        output='screen'
    )
    
    cmd_vel2qita = Node(
            package="topic_tools",
            executable="relay",
            # arguments = ['/cmd_vel' , new_cmd_vel],
            arguments=[
                '/cmd_vel',
                new_cmd_vel,
                '--input-qos', 'reliability=reliable,durability=volatile,depth=10',
                '--output-qos', 'reliability=best_effort,durability=volatile,depth=5'
            ],
            name="relay_node"
    )
    
    return launch.LaunchDescription([
        # 添加偏移参数声明
        offset_x_arg,
        offset_y_arg,
        offset_z_arg,
        
        urdf2tf,
        mavros2odom,
        odom2tf, 
        cmd_vel_new,
        cmd_vel2qita,
      
    ])